#针对蓝队
import RPi.GPIO as GPIO
import time
#bgr8转jpeg格式
import enum
import cv2

def bgr8_to_jpeg(value, quality=75):
    return bytes(cv2.imencode('.jpg', value)[1])

#摄像头组件显示
import traitlets
import ipywidgets.widgets as widgets
#线程功能操作库
import threading
import inspect
import ctypes
import numpy as np

origin_widget = widgets.Image(format='jpeg', width=320, height=240)
mask_widget = widgets.Image(format='jpeg',width=320, height=240)
result_widget = widgets.Image(format='jpeg',width=320, height=240)

# create a horizontal box container to place the image widget next to eachother
image_container = widgets.HBox([origin_widget, mask_widget, result_widget])

display(image_container)

#线程相关函数
def _async_raise(tid, exctype):
    """raises the exception, performs cleanup if needed"""
    tid = ctypes.c_long(tid)
    if not inspect.isclass(exctype):
        exctype = type(exctype)
    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))
    if res == 0:
        raise ValueError("invalid thread id")
    elif res != 1:
        # """if it returns a number greater than one, you're in trouble,
        # and you should call it again with exc=NULL to revert the effect"""
        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)
        
def stop_thread(thread):
    _async_raise(thread.ident, SystemExit)

cap = cv2.VideoCapture(0)
cap.set(3, 640)
cap.set(4, 480)
cap.set(5, 120)  #设置帧率
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
cap.set(cv2.CAP_PROP_BRIGHTNESS, 20) #设置亮度 -64 - 64  0.0
cap.set(cv2.CAP_PROP_CONTRAST, 20)   #设置对比度 -64 - 64  2.0
# image.set(cv2.CAP_PROP_EXPOSURE, 156)  #设置曝光值 1.0 - 5000  156.0

# 默认选择红色的，想识别其他请注释下面红色区间代码，放开后面其他区间代码段
# 红色区域
#color_lower = np.array([0, 43, 46])
#color_upper = np.array([10, 255, 255])

# #绿色区间
# color_lower = np.array([35, 43, 46])
# color_upper = np.array([77, 255, 255])

# #蓝色区间
#color_lower=np.array([100, 43, 46])
#color_upper = np.array([124, 255, 255])

# #黄色区间
# color_lower = np.array([26, 43, 46])
# color_upper = np.array([34, 255, 255])

#小车电机引脚定义
IN1 = 20
IN2 = 21
IN3 = 19
IN4 = 26
ENA = 16
ENB = 13

#小车按键定义
key = 8

#循迹红外引脚定义
#TrackSensorLeftPin1 TrackSensorLeftPin2 TrackSensorRightPin1 TrackSensorRightPin2
#      3                 5                  4                   18
TrackSensorLeftPin1  =  3   #定义左边第一个循迹红外传感器引脚为3口
TrackSensorLeftPin2  =  5   #定义左边第二个循迹红外传感器引脚为5口
TrackSensorRightPin1 =  4   #定义右边第一个循迹红外传感器引脚为4口
TrackSensorRightPin2 =  18  #定义右边第二个循迹红外传感器引脚为18口

#设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)

#忽略警告信息
GPIO.setwarnings(False)

#电机引脚初始化为输出模式
#按键引脚初始化为输入模式
#寻迹引脚初始化为输入模式
def init():
    global pwm_ENA
    global pwm_ENB
    GPIO.setup(ENA,GPIO.OUT,initial=GPIO.HIGH)
    GPIO.setup(IN1,GPIO.OUT,initial=GPIO.LOW)
    GPIO.setup(IN2,GPIO.OUT,initial=GPIO.LOW)
    GPIO.setup(ENB,GPIO.OUT,initial=GPIO.HIGH)
    GPIO.setup(IN3,GPIO.OUT,initial=GPIO.LOW)
    GPIO.setup(IN4,GPIO.OUT,initial=GPIO.LOW)
    GPIO.setup(key,GPIO.IN)
    GPIO.setup(TrackSensorLeftPin1,GPIO.IN)
    GPIO.setup(TrackSensorLeftPin2,GPIO.IN)
    GPIO.setup(TrackSensorRightPin1,GPIO.IN)
    GPIO.setup(TrackSensorRightPin2,GPIO.IN)
    #设置pwm引脚和频率为2000hz
    pwm_ENA = GPIO.PWM(ENA, 2000)
    pwm_ENB = GPIO.PWM(ENB, 2000)
    pwm_ENA.start(0)
    pwm_ENB.start(0)
	
#小车前进	
def run(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

#小车后退
def back(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)
	
#小车左转	
def left(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

#小车右转
def right(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)
	
def runone(delaytime):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(35)
    pwm_ENB.ChangeDutyCycle(35)
    time.sleep(delaytime)

#小车后退
def backone(delaytime):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(35)
    pwm_ENB.ChangeDutyCycle(35)
    time.sleep(delaytime)
    
#小车原地左转
def spin_left(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

#小车原地右转
def spin_right(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

#小车停止	
def brake():
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)

#按键检测
def key_scan():
    while GPIO.input(key):
        pass
    while not GPIO.input(key):
        time.sleep(0.01)
        if not GPIO.input(key):
            time.sleep(0.01)
        while not GPIO.input(key):
	        pass

def judgecross():
    TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
    TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
    TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
    TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
    if (TrackSensorLeftValue1 == False and TrackSensorRightValue2 == False):
        return 1
    elif(TrackSensorLeftValue1 == False or TrackSensorRightValue2 == False):
        return 2
    else:
        return 0
    
#小车左转函数
def turn_left(leftspeed, rightspeed,duration):
    start_time = time.time()
    while True:
        spin_left(leftspeed,rightspeed)
        current_time = time.time()
        if current_time -  start_time >= time: #设定旋转时间，只是参数
            break
#小车右转函数
def turn_right():
    start_time = time.time()
    while True:
        if TrackSensorLeftValue1 == False:
            spin_right(80, 80)
        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
            run(100, 100)
        current_time = time.time()
        if current_time -  start_time >= 1: #设定旋转时间，只是参数
            break    
#延时2s	
time.sleep(2)

def Color_Recongnize():
    
    global is_detected
    is_detected = 2
    
    while(1):
        #获取视频帧
        ret, frame = cap.read()
        origin_widget.value = bgr8_to_jpeg(frame)

        #将帧转换为HSV模型
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

        #设定范围
        lower_blue = np.array([100, 50, 50])
        upper_blue = np.array([130, 255, 255])
        lower_yellow = np.array([20, 100, 46])#v:46
        upper_yellow = np.array([40, 255, 255])
        lower_green = np.array([20, 43, 46])
        upper_green = np.array([90, 255, 255])# 77 255 255
        lower_red = np.array([0, 43, 46])
        upper_red = np.array([10, 255, 255])
        # 获取蓝色区域的掩码
        blue_mask = cv2.inRange(hsv,lower_blue,upper_blue)
        # 获取黄色区域的掩码
        yellow_mask = cv2.inRange(hsv,lower_yellow,upper_yellow)
        # 获取绿色区域的掩码
        green_mask = cv2.inRange(hsv,lower_green,upper_green)
        #获取红色区域的掩码
        red_mask = cv2.inRange(hsv,lower_red,upper_red)
        # 合并两种掩码
        #蓝黄
        blue_yellow_mask = cv2.bitwise_or(blue_mask, yellow_mask)
        #蓝绿
        blue_green_mask = cv2.bitwise_or(blue_mask, green_mask)
        #red_yellow_mask = cv2.bitwise_or(red_mask,yellow_mask)
        mask = cv2.bitwise_or(blue_yellow_mask,blue_green_mask)
        mask_widget.value = bgr8_to_jpeg(mask)
        
        # detect blue 将mask于原视频帧进行按位与操作，则会把mask中的白色用真实的图像替换：
        res = cv2.bitwise_and(frame, frame, mask=mask)
        #cv2.imshow('Result', res)
        result_widget.value = bgr8_to_jpeg(res)
        # 判断宝藏真假
        
        if ((cv2.countNonZero(yellow_mask) > 10) and cv2.countNonZero(blue_mask) > 100):
            is_detected = 1
        elif (cv2.countNonZero(blue_mask) > 100 and cv2.countNonZero(green_mask) >10):
            is_detected = 3
        elif (cv2.countNonZero(red_mask) > 100):
            is_detected = 3
        else:
            is_detected = 2
            
        time.sleep(0.01)


    cap.release()
    #cv2.destroyAllWindows()
#启动进程
thread1 = threading.Thread(target=Color_Recongnize)
thread1.setDaemon(True)
thread1.start()
#路径规划需要的变量 l-左转 r-右转 0-直走 3-
turn_directions0 = ['1','1','2','2','1','2','1','1','1']
turn_directions1 = ['3','2','1','2','1','2','1','1','3','1']#红色赛道
turn_directions2 = ['2','3','3','2','3']
turn_directions3 = ['2','1','2','2']
turn_directions4 = ['1','1','1','2','3','1','2','1','1']
turn_directions5 = ['2','2','2','1','2','1','1','2','2']
all_directions = [turn_directions0, turn_directions1, turn_directions2,turn_directions3,turn_directions4,turn_directions5]
#try/except语句用来检测try语句块中的错误，
#从而让except语句捕获异常信息并处理。

try:
    init()
    key_scan()
    i = 0 
    while(i <= len(all_directions)):
        total_crossroads = len(all_directions[i])
    #路径规划转化为小车行进
        current_crossroad = 0
        while current_crossroad < total_crossroads:
            #循迹 未遇到路口直行，遇到路口跳出下循环
            while True:
                TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
                TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
                TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                #循迹部分
                if (TrackSensorLeftValue1 == False or TrackSensorRightValue2 == False):
                    break
                elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                    run(14,14)#14
                    #路口判断
                elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                    left(0,35)
                elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                    right(35,0)
            start_time = time.time()
            #路口转弯
            while True:
                TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
                TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
                TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                if(all_directions[i][current_crossroad] == '1'):
                    if TrackSensorLeftValue1 == False:
                        spin_left(40, 40)#40
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(20, 20)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0,35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35, 0)
                    current_time1 = time.time()
                    if current_time1 - start_time >= 1:
                        break
                elif(all_directions[i][current_crossroad]== '2'):
                    if TrackSensorRightValue2 == False:
                        spin_right(40, 40)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(20, 20)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0,35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35, 0)
                    current_time2 = time.time()
                    if current_time2 - start_time >= 1:
                        break
            
                elif(all_directions[i][current_crossroad]== '3'):
                    if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(20,20)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0,35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35, 0)
                    current_time3 = time.time()
                    if current_time3 - start_time >=0.3:
                        break
            current_crossroad += 1
        #颜色识别和撞击宝藏
        print('loop')
        while True:
            if is_detected == 2:
                print('no')
                while True:
                    TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
                    TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
                    TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                    TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                    if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(5,5)#14
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0,35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35,0)
                    if ((is_detected ==1) or (is_detected == 3)):
                        break
            if is_detected == 1:
                print('true')
                runone(0.2)
                backone(0.4)
                start_time1_1 = time.time()
                while True:
                    TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
                    TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
                    TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                    TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                    spin_left(40,40)
                    current_time1_1 = time.time()
                    if current_time1_1 - start_time1_1 >= 0.87:
                        break
                start_time1_2 = time.time()                
                while True:
                    TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
                    TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
                    TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                    TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)   
                    if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(7,7)#14
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0,35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35,0)
                    current_time1_2 = time.time()
                    if current_time1_2 - start_time1_2 >= 0.4:
                        break
            elif is_detected == 3:
                print('false')
                start_time1_1 = time.time()
                backone(0.1)
                while True:
                    TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
                    TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
                    TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                    TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)                
                    spin_left(40,40)
                    current_time1_1 = time.time()
                    if current_time1_1 - start_time1_1 >= 0.87:
                        break
                start_time1_2=time.time()
                while True:
                    TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
                    TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
                    TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                    TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)   
                    if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(5,5)#14
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0,35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35,0)
                    current_time1_2 = time.time()
                    if current_time1_2 - start_time1_2 >= 0.4:
                        break
            break 
        i=i+1
except KeyboardInterrupt:
    pass
pwm_ENA.stop()
pwm_ENB.stop()
GPIO.cleanup()


